package pteam;


import java.util.*;

import simulator.Actor;
import simulator.BeaconSensor;
import simulator.Robot;
import simulator.Sensor;
import simulator.Simulator;
import simulator.BumpSensor;
public class PlanCActor  implements Actor {
  public final int TOLERANCE = 5;
  public final int TOOCLOSE = 500;
  public final int FARDIST = 2;
  public final int FARANGLE = 1;
  public final int NOTFARDIST = 5;
  public final int NOTFARANGLE = 1;
  public final int BACKDIST = 15;
  public final int INCREMENT = 1;
  public boolean tooClose ;
  public boolean leftLong;
  public boolean bumped;
  public boolean beacon;  //L C R
  public double dif;
  public int pos = 45;
  public int dir = 1;
  
  public String getMaze() {
    return "GoldGen";
  }

  public void act(Robot r) {
	  if(bumped){
		  r.goBackward();
		  r.waitDistance(15);
		  if(Math.random() < .5)
			  r.turnRight();
		  else
			  r.turnLeft();
		  r.waitAngle((int) (45*(Math.random()-.5) + 90));
		  return;
	  }else{
	  if(!beacon && false){
		  if(!leftLong)
			  r.turnRight();
		  else
			  r.turnLeft();
		  r.waitAngle(Math.abs((int)(.5*dif)));
	  }
	  //}else{
		  r.goForward();
		  r.waitDistance(1);
      }
	   /*
      //r.goBackward();
      //r.waitDistance(1);
      //r.turnLeft();
      //r.waitAngle(2);
      //r.turnRight();
      //r.waitAngle(2);
      //try { Thread.sleep(125); } catch(Exception e) {}
      
    		Simulator.out.println("-- Rotate --");
		r.getServos()[0].turn(INCREMENT*dir);
		r.getServos()[1].turn(-1*INCREMENT*dir);
		pos+=dir;
		if(pos>=90 || pos<=0){
			dir *= -1;
			//r.getServos()[0].turn(-90);
			//r.getServos()[1].turn(90);
			//pos=45;
			//r.goForward();
			//r.waitDistance(10);
		}*/
  }
    
  public void updateSensors(Sensor[] s) {
   bumped = (s[0].getState()[0]==BumpSensor.PRESSED);
   beacon = false; /*s[6].getState()[0] == BeaconSensor.INBOTH || 
   					s[4].getState()[0] == s[2].getState()[0];*/
   dif = s[4].getState()[0] - s[2].getState()[0];
   tooClose = (s[1].getState()[0] < TOOCLOSE ||
		   (s[4].getState()[0] < TOOCLOSE/2 || s[2].getState()[0] < TOOCLOSE/2));
   leftLong = s[4].getState()[0] > s[2].getState()[0];
   /*String output = new String();
   for(int i=0; i<s.length; i++){
     try{
       //System.out.println(s);
       //System.out.println(i+": " +s[i].getState()[0]);
         output = output + s[i].getState()[0] + "	";
     }catch(Exception e){
     	e.printStackTrace();
     }
   }
   System.out.println(output);
   */          
   }
}